#include "main.h"

uint16_t xdata task1;
uint16_t xdata task2;
uint16_t xdata task3;

static motor_info_t *g_motor_list = NULL;

void delay_ms(u16 ms)
{
    unsigned char i, j;
        while(ms--){
        _nop_();
        _nop_();
        i = 44;
        j = 8;
        do
        {
            while (--j);
        } while (--i);
    }
}

/**
 * M1 P00-P03   步进
 * M2 P04-P07   步进
 * M3 P40-P43   步进
 * M4 P44-P47   步进
 * E1 P50-P53   步进
 * E2 P54       舵机
 */
void write_motor_func(motor_name_e motor_num, uint8_t sign)
{
    uint8_t buf = 0;
    switch(motor_num) {
        case MOTOR_M1: {
            buf= P0;
            buf &= 0xf0;
            P0 = (buf | sign);
        } break;
        case MOTOR_M2: {
            buf= P0;
            buf &= 0x0f;
            P0 = (buf | (sign << 4));
        } break;
        case MOTOR_M3: {
            buf= P4;
            buf &= 0xf0;
            P4 = (buf | sign);
        } break;
        case MOTOR_M4: {
            buf= P4;
            buf &= 0x0f;
            P4 = (buf | (sign << 4));
        } break;
        case MOTOR_E1: {
            buf= P5;
            buf &= 0xf0;
            P5 = (buf | sign);
        } break;
    }
}

void User_motor_Init(void)
{
    uint8_t i = 0;
	g_motor_list = get_motor_list();
	robot_init(write_motor_func);

    for (i = 0; i < MOTOR_MAX_NUM; i++)
    {
		g_motor_list[i].work_mode = MOTOR_STOP_MODE;
		g_motor_list[i].idle_mode = MOTOR_STOP_MODE;
        g_motor_list[i].freq_div = 4;
        g_motor_list[i].cur_step_num = 0;
    }
    g_motor_list[MOTOR_M1].freq_div = 0;
}

void motor_control()
{
    if (C11 == 0) {
        g_motor_list[MOTOR_M1].dir = 1;
        g_motor_list[MOTOR_M1].work_mode = MOTOR_MANUAL_MODE;
        LED1 = 1;
    } else if (C12 == 0) {
        g_motor_list[MOTOR_M1].dir = 0;
        g_motor_list[MOTOR_M1].work_mode = MOTOR_MANUAL_MODE;
        LED1 = 1;
    } else {
        g_motor_list[MOTOR_M1].work_mode = MOTOR_STOP_MODE;
        LED1 = 0;
    }

    if (C21 == 0) {
        g_motor_list[MOTOR_M2].dir = 1;
        g_motor_list[MOTOR_M2].work_mode = MOTOR_MANUAL_MODE;
        LED2 = 1;
    } else if (C22 == 0) {
        g_motor_list[MOTOR_M2].dir = 0;
        g_motor_list[MOTOR_M2].work_mode = MOTOR_MANUAL_MODE;
        LED2 = 1;
    } else {
        g_motor_list[MOTOR_M2].work_mode = MOTOR_STOP_MODE;
        LED2 = 0;
    }

    if (C31 == 0) {
        g_motor_list[MOTOR_M3].dir = 1;
        g_motor_list[MOTOR_M3].work_mode = MOTOR_MANUAL_MODE;
        LED3 = 1;
    } else if (C32 == 0) {
        g_motor_list[MOTOR_M3].dir = 0;
        g_motor_list[MOTOR_M3].work_mode = MOTOR_MANUAL_MODE;
        LED3 = 1;
    } else {
        g_motor_list[MOTOR_M3].work_mode = MOTOR_STOP_MODE;
        LED3 = 0;
    }

    if (C41 == 0) {
        g_motor_list[MOTOR_M4].dir = 1;
        g_motor_list[MOTOR_M4].work_mode = MOTOR_MANUAL_MODE;
        LED4 = 1;
    } else if (C42 == 0) {
        g_motor_list[MOTOR_M4].dir = 0;
        g_motor_list[MOTOR_M4].work_mode = MOTOR_MANUAL_MODE;
        LED4 = 1;
    } else {
        g_motor_list[MOTOR_M4].work_mode = MOTOR_STOP_MODE;
        LED4 = 0;
    }

    if (C51 == 0) {
        g_motor_list[MOTOR_E1].dir = 1;
        g_motor_list[MOTOR_E1].work_mode = MOTOR_MANUAL_MODE;
    } else if (C52 == 0) {
        g_motor_list[MOTOR_E1].dir = 0;
        g_motor_list[MOTOR_E1].work_mode = MOTOR_MANUAL_MODE;
    } else {
        g_motor_list[MOTOR_E1].work_mode = MOTOR_STOP_MODE;
    }
}

void uart_control_motor(uint8_t cmd)
{
    uint8_t i = 0;
    printf("cmd:%x\r\n", cmd);
    switch (cmd)
    {
    case 0x44:      //左转
        g_motor_list[MOTOR_M1].dir = 1;
        g_motor_list[MOTOR_M1].work_mode = MOTOR_MANUAL_MODE;
        LED1 = 1;
        break;
    case 0x43:      //右转
        g_motor_list[MOTOR_M1].dir = 0;
        g_motor_list[MOTOR_M1].work_mode = MOTOR_MANUAL_MODE;
        LED1 = 1;
        break;
    case 0x46:      
        g_motor_list[MOTOR_M2].dir = 0;
        g_motor_list[MOTOR_M2].work_mode = MOTOR_MANUAL_MODE;
        LED2 = 1;
        break;
    case 0x15:
        g_motor_list[MOTOR_M2].dir = 1;
        g_motor_list[MOTOR_M2].work_mode = MOTOR_MANUAL_MODE;
        LED2 = 1;
        break;
    case 0x47:
        g_motor_list[MOTOR_M3].dir = 0;
        g_motor_list[MOTOR_M3].work_mode = MOTOR_MANUAL_MODE;
        LED3 = 1;
        break;
    case 0x09:
        g_motor_list[MOTOR_M3].dir = 1;
        g_motor_list[MOTOR_M3].work_mode = MOTOR_MANUAL_MODE;
        LED3 = 1;
        break;
    default:
        break;
    }
    task1 = 0;
}

void main()
{
    uint8_t i = 0;
    P0M0 = 0x00;
    P0M1 = 0x00;
    P1M0 = 0x00;
    P1M1 = 0x00;
    P2M0 = 0x00;
    P2M1 = 0x00;
    P3M0 = 0x00;
    P3M1 = 0x00;
    P4M0 = 0x00;
    P4M1 = 0x00;
    P5M0 = 0x00;
    P5M1 = 0x00;
    
    Uart_init();
    Timer0Init();
    Timer4Init();
    delay_ms(1);
    EA = 1;
    
    printf("UartSendStrTest\r\n");
    printf("time:%s\r\n",__TIME__);

	Uart1SendStr("uart1\r\n",5);
	Uart2SendStr("uart2\r\n",5);
    
    User_motor_Init();
    while (1)
    {
        if(task1 >= 110){
            task1 = 0;
            motor_control();
        }

        if(task2 >= 500){
            task2 = 0;
            LED5 = ~LED5;
        }

        if(task3 >= 10) {
            task3 = 0;
            robot_run_tick();
        }

        if(rx1_sta & 0x80)Uart1_handle();
		if(rx2_sta & 0x80) {
            uart_control_motor(rx2_buf[0]);
            Uart2_handle();
        }
    }
}
